/*
   IMU visualize
   convert custom_msg to pose
   by Kyohei
*/

#include <ros/ros.h>
#include <sensor_msgs/Imu.h>
#include "navigation/imu.h"

using namespace navigation;

ros::Subscriber imu_sub;
ros::Publisher imu_pub;

void callback(const imu::ConstPtr &msg) {
  sensor_msgs::Imu imu;
  imu.orientation.x = msg->comp_x;
  imu.orientation.y = msg->comp_y;
  imu.orientation.z = msg->comp_z;

  imu.angular_velocity.x = msg->gyro_x;
  imu.angular_velocity.y = msg->gyro_y;
  imu.angular_velocity.z = msg->gyro_z;

  imu.linear_acceleration.x = msg->acc_x;
  imu.linear_acceleration.y = msg->acc_y;
  imu.linear_acceleration.z = msg->acc_z;

  imu_pub.publish(imu);
}

int main(int ac, char **av) {
  ros::init(ac, av, "imu_visualize");
  ros::NodeHandle n;
  imu_sub = n.subscribe("/imu", 1000, callback);
  imu_pub = n.advertise<sensor_msgs::Imu>("/imu_visualize", 1000);

  ros::spin();
}
